// -----------------------------------------------------------------------------
// This file was autogenerated by symforce from template:
//     backends/cpp/templates/function/FUNCTION.h.jinja
// Do NOT modify by hand.
// -----------------------------------------------------------------------------

#pragma once

#include <matrix/math.hpp>

namespace sym {

/**
 * This function was autogenerated from a symbolic function. Do not modify by hand.
 *
 * Symbolic function: yaw_est_compute_measurement_update
 *
 * Args:
 *     P: Matrix33
 *     vel_obs_var: Scalar
 *     epsilon: Scalar
 *
 * Outputs:
 *     S_inv: Matrix22
 *     S_det_inv: Scalar
 *     K: Matrix32
 *     P_new: Matrix33
 */
template <typename Scalar>
void YawEstComputeMeasurementUpdate(const matrix::Matrix<Scalar, 3, 3>& P, const Scalar vel_obs_var,
                                    const Scalar epsilon,
                                    matrix::Matrix<Scalar, 2, 2>* const S_inv = nullptr,
                                    Scalar* const S_det_inv = nullptr,
                                    matrix::Matrix<Scalar, 3, 2>* const K = nullptr,
                                    matrix::Matrix<Scalar, 3, 3>* const P_new = nullptr) {
  // Total ops: 60

  // Input arrays

  // Intermediate terms (15)
  const Scalar _tmp0 = P(1, 1) + vel_obs_var;
  const Scalar _tmp1 = P(0, 0) + vel_obs_var;
  const Scalar _tmp2 = -P(0, 1) * P(1, 0) + _tmp0 * _tmp1;
  const Scalar _tmp3 =
      Scalar(1.0) /
      (_tmp2 + epsilon * (2 * math::min<Scalar>(0, (((_tmp2) > 0) - ((_tmp2) < 0))) + 1));
  const Scalar _tmp4 = _tmp0 * _tmp3;
  const Scalar _tmp5 = P(1, 0) * _tmp3;
  const Scalar _tmp6 = P(0, 1) * _tmp3;
  const Scalar _tmp7 = _tmp1 * _tmp3;
  const Scalar _tmp8 = -P(0, 1) * _tmp5;
  const Scalar _tmp9 = P(0, 0) * _tmp4 + _tmp8;
  const Scalar _tmp10 = -P(1, 1) * _tmp5 + _tmp0 * _tmp5;
  const Scalar _tmp11 = P(2, 0) * _tmp4 - P(2, 1) * _tmp5;
  const Scalar _tmp12 = -P(0, 0) * _tmp6 + _tmp1 * _tmp6;
  const Scalar _tmp13 = P(1, 1) * _tmp7 + _tmp8;
  const Scalar _tmp14 = -P(2, 0) * _tmp6 + P(2, 1) * _tmp7;

  // Output terms (4)
  if (S_inv != nullptr) {
    matrix::Matrix<Scalar, 2, 2>& _s_inv = (*S_inv);

    _s_inv(0, 0) = _tmp4;
    _s_inv(1, 0) = -_tmp5;
    _s_inv(0, 1) = -_tmp6;
    _s_inv(1, 1) = _tmp7;
  }

  if (S_det_inv != nullptr) {
    Scalar& _s_det_inv = (*S_det_inv);

    _s_det_inv = _tmp3;
  }

  if (K != nullptr) {
    matrix::Matrix<Scalar, 3, 2>& _k = (*K);

    _k(0, 0) = _tmp9;
    _k(1, 0) = _tmp10;
    _k(2, 0) = _tmp11;
    _k(0, 1) = _tmp12;
    _k(1, 1) = _tmp13;
    _k(2, 1) = _tmp14;
  }

  if (P_new != nullptr) {
    matrix::Matrix<Scalar, 3, 3>& _p_new = (*P_new);

    _p_new(0, 0) = -P(0, 0) * _tmp9 + P(0, 0) - P(1, 0) * _tmp12;
    _p_new(1, 0) = 0;
    _p_new(2, 0) = 0;
    _p_new(0, 1) = -P(0, 1) * _tmp9 + P(0, 1) - P(1, 1) * _tmp12;
    _p_new(1, 1) = -P(0, 1) * _tmp10 - P(1, 1) * _tmp13 + P(1, 1);
    _p_new(2, 1) = 0;
    _p_new(0, 2) = -P(0, 2) * _tmp9 + P(0, 2) - P(1, 2) * _tmp12;
    _p_new(1, 2) = -P(0, 2) * _tmp10 - P(1, 2) * _tmp13 + P(1, 2);
    _p_new(2, 2) = -P(0, 2) * _tmp11 - P(1, 2) * _tmp14 + P(2, 2);
  }
}  // NOLINT(readability/fn_size)

// NOLINTNEXTLINE(readability/fn_size)
}  // namespace sym
